#pragma comment( linker, "/subsystem:\"windows\" /entry:\"mainCRTStartup\"" ) 

#include "../GL/glut.h"
#include <stdlib.h>
#include <cmath>
#include <vector>
#include <string>

#include "../mtxlib/mtxlib.h"

#include "KinematicChain.h"
#include "Camera.h"
#include "Effector.h"
#include "Utilities.h"
#include "Solver.h"
#include "Grid.h"
#include "Renderable.h"
#include "FlatObject.h"
#include "GLConsole.h"

#include <windows.h>


std::vector<Renderable *> renderableObjs;
std::vector<std::string> fileNames;

#define EFFDATANUM 1000

std::vector<std::pair<mtxlib::vector4, mtxlib::vector4> > effectorData(EFFDATANUM);
int currentFile = 0;
bool solveImmedietely = false;


extern bool useAproxSolver;
extern bool recalculateAproxSolver;
extern bool firstSolution;

struct 
{
	bool left;
	bool right;
	bool middle;
	int ox, oy;                
	float rx, ry;
} mouseState;


void InitGL()              
{
	glClearColor(.9f, .9f, 1.0f, 1.0f);            
	glClearDepth(1.0);                               
	glDepthFunc(GL_LESS);                                
	glEnable(GL_DEPTH_TEST);                      
	glShadeModel(GL_SMOOTH);                      

	GLfloat ambiref[] = {0.3f, 0.3f, 0.3f, 1.0f};
	GLfloat diffref[] = {0.5f, 0.5f, 0.5f, 1.0f};
	GLfloat specref[] = {1.0f, 1.0f, 1.0f, 1.0f};
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, ambiref);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, diffref);
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, specref);
	glMateriali(GL_FRONT_AND_BACK, GL_SHININESS, 128);
	glEnable(GL_COLOR_MATERIAL);
	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);

	GLfloat ambientLight[] = {0.1f, 0.1f, 0.1f, 1.0f};
	GLfloat diffuseLight[] = {0.3f, 0.3f, 0.3f, 1.0f};
	GLfloat specularLight[] = {0.9f, 0.3f, 0.3f, 1.0f};
	GLfloat lightPos[] = {0, 0, 100, 0}; //hardkod
	glEnable(GL_LIGHTING);
	glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight);
	glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight);
	glLightfv(GL_LIGHT0, GL_POSITION, lightPos);
	glEnable(GL_LIGHT0);

	glFrontFace(GL_CCW);
	glCullFace(GL_BACK);
	glEnable(GL_NORMALIZE);

	globalUtils.quadratic = gluNewQuadric();			
	gluQuadricNormals(globalUtils.quadratic, GLU_SMOOTH);	
	gluQuadricTexture(globalUtils.quadratic, GL_TRUE);

	glMatrixMode(GL_MODELVIEW);
}


template<bool Pic>
void DisplayChain(Renderable *rnd)
{
	KinematicChain *kc = (KinematicChain *)rnd;
	
	float rotationalRadius = 0.2;

	glPushMatrix();
	for(int i = 0; i < DEG_FR; i++){
		glPushMatrix();
		glLineWidth(4.0);
		glBegin(GL_LINES);
		glColor3f(0, 0.3, 0.4);
		glVertex3f(0, 0, 0);
		glVertex3f(kc->V(i).x, kc->V(i).y, kc->V(i).z);
		glEnd();
		glLineWidth(1.0);
		if(kc->JT(i) == KinematicChain::JointType::Prismatic)
			glMultMatrixf((float*)(&(kc->TR(i))));
		else
			glMultMatrixf((float*)(&(kc->TR(i))));
		if(kc->IsSelected && kc->SelectedJoint() == i && !Pic)
		{
			//glScalef(1.1, 1.1, 1.0);
			glClearStencil(0);
			glClear(GL_STENCIL_BUFFER_BIT);
			glEnable(GL_STENCIL_TEST);
			glStencilFunc(GL_ALWAYS, 1, -1);
			glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
			if(kc->JT(i) == KinematicChain::JointType::Prismatic)
			{
				if(Pic)
					glColor3f(kc->ColorId/255.0, i/255.0, 0);
				else
					glColor3f(1, 1, 0);
				glPushMatrix();
				if(kc->A(i) < 0)
					glScalef(1, 1, -1);
				gluCylinder(globalUtils.quadratic, 
					min(0.3, .03/log(1 + fabs(kc->A(i)))), 
					min(0.2, .02/log(1 + fabs(kc->A(i)))), 
					fabs(kc->A(i)), 
					16, 16);
				glPopMatrix();
			}
			else //if(kc->JT == Rotational
			{
				if(Pic)
					glColor3f(kc->ColorId/255.0, i/255.0, 0);
				else

					glColor3f(1, 0, 1);
				glutSolidTorus(rotationalRadius / 3, rotationalRadius, 5, 10);
			}
			glStencilFunc(GL_NOTEQUAL, 1, -1);
			glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
			glLineWidth(5);
			glPolygonMode(GL_FRONT, GL_LINE);
			if(kc->JT(i) == KinematicChain::JointType::Prismatic)
			{
				glColor3f(0, 0, 0);
				glPushMatrix();
				if(kc->A(i) < 0)
					glScalef(1, 1, -1);
				gluCylinder(globalUtils.quadratic, 
					min(0.3, .03/log(1 + fabs(kc->A(i)))), 
					min(0.2, .02/log(1 + fabs(kc->A(i)))), 
					fabs(kc->A(i)), 
					16, 16);
				glPopMatrix();
			}
			else //if(kc->JT == Rotational
			{
				glColor3f(0, 0, 0);
				glutSolidTorus(rotationalRadius / 3, rotationalRadius, 5, 10);
			}
			glDisable(GL_STENCIL_TEST);
			glPolygonMode(GL_FRONT, GL_FILL);
			glLineWidth(1);
		}
		else if(kc->JT(i) == KinematicChain::JointType::Prismatic)
		{
			if(Pic)
				glColor3f(kc->ColorId/255.0, i/255.0, 0);
			else
				glColor3f(1, 1, 0);
			glPushMatrix();
			if(kc->A(i) < 0)
				glScalef(1, 1, -1);
			gluCylinder(globalUtils.quadratic, 
				min(0.3, .03/log(1 + fabs(kc->A(i)))), 
				min(0.2, .02/log(1 + fabs(kc->A(i)))), 
				fabs(kc->A(i)), 
				16, 16);
			glPopMatrix();
		}
		else //if(kc->JT == Rotational
		{
			if(Pic)
				glColor3f(kc->ColorId/255.0, i/255.0, 0);
			else
				glColor3f(1, 0, 1);
			glutSolidTorus(rotationalRadius / 3, rotationalRadius, 5, 10);
		}
		glPopMatrix();
		glMultMatrixf((float*)(&(kc->TRP(i))));
		rotationalRadius -= rotationalRadius/6;
		//DisplayFrame();
	}
	glLineWidth(3.0);
	DisplayFrameWF();
	glLineWidth(1.0);
	glPopMatrix();
}


void DisplayChain2(KinematicChain *kc)
{
	mtxlib::matrix44 mat = mtxlib::IdentityMatrix44();

	for(int i = 0; i < DEG_FR; i++)
	{
		DisplayFrameWF(mat);
		mat = mat * kc->TRP(i);
	}
}


template<bool Pic>
void DisplayEffector(Renderable *rnd)
{
	Effector *ef = (Effector *)rnd;
	glPushMatrix();
	
	glMultMatrixf((float*)(&ef->GetMatrix()));
	if(!Pic)
	{
		if(ef->IsSelected)
		{
			glPushMatrix();
			glScalef(1.1, 1.1, 1.1);
			glClearStencil(0);
			glClear(GL_STENCIL_BUFFER_BIT);
			glEnable(GL_STENCIL_TEST);
			glStencilFunc(GL_ALWAYS, 1, -1);
			glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
			DisplayFrame();
			glStencilFunc(GL_NOTEQUAL, 1, -1);
			glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
			glLineWidth(5);
			glPolygonMode(GL_FRONT, GL_LINE);
			DisplayFrame(0.0, 0, 0);
			glDisable(GL_STENCIL_TEST);
			glPolygonMode(GL_FRONT, GL_FILL);
			glLineWidth(1);
			glPopMatrix();
		}
		else
		{
			DisplayFrame();
		}
	}
	else
	{
		glDisable(GL_LIGHTING);
		DisplayFrame(ef->ColorId/255.0, 0, 0);
		glEnable(GL_LIGHTING);
	}

	glPopMatrix();
}


void DrawGLScene()
{
	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);       
	glLoadIdentity();   
	
	mtxlib::matrix44 mat = Camera::getSingleton().GetMatrix();
	glMultMatrixf((float*)&mat); 

	for(size_t i = 0; i < renderableObjs.size(); i++)
	{
		if(renderableObjs[i]->Visible){
			renderableObjs[i]->Render();
		}
	}

	glutSwapBuffers();
}


void ProcessPicing(int x, int y)
{
	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    glDisable(GL_LIGHTING);

	for(size_t i = 0; i < renderableObjs.size(); i++)
	{
		renderableObjs[i]->PicRender();
	}

	unsigned char pixel[3];
	GLint viewport[4];
	glGetIntegerv(GL_VIEWPORT, viewport);

	glReadPixels(x, viewport[3] - y, 1, 1, GL_RGB, GL_UNSIGNED_BYTE, pixel);

	for(size_t i = 0; i < renderableObjs.size(); i++)
	{
		renderableObjs[i]->IsSelected = false;
		if(renderableObjs[i]->ColorId == pixel[0])
		{
			renderableObjs[i]->ProcessSelection(pixel[1]);
		}	
	}
	glEnable(GL_LIGHTING);
}


template<bool Pic>
void DrawGrid(Renderable *)
{
	if(!Pic){
		glColor3f(0.3, 0.3, 0.3);
		DrawGrid(-10, 10, -10, 10, 0.3);
	}
	else
	{
		DrawGrid(-10, 10, -10, 10, 0.3);
	}
}


void KeyPressed(unsigned char key, int x, int y)
{
	if (key == '')
	{
		//glutDestroyWindow(window);

		exit(0);
		//glut
	}
	else if(key >= 'a' && key < 'a' + 2*DEG_FR)
	{
		if((key - 'a') % 2 == 0)
		{
			KinematicChain::getSingleton().A((key-'a')/2) += 0.1; //hardkod
		}
		else
		{
			KinematicChain::getSingleton().A((key-'a')/2) -= 0.1; //hardkod
		}
	}
	else if(key == 's')
	{
		SolveReverseKinematicProblem(KinematicChain::getSingletonPtr(), Effector::getSingletonPtr());
	}
	else if(key == 'w')
	{
		char buf[255];
		KinematicChain *kc = KinematicChain::getSingletonPtr();
		GLConsole *console = GLConsole::getSingletonPtr();
		
		for(int i = 0; i < DEG_FR; i++)
		{
			printf("%d: %.2f  ", i, kc->A(i));
		}
		printf("\n");

		console->PrintLine("");
		console->PrintLine("Wartosci dla poszczegolnych zlaczy:");
		for(int i = 0; i < DEG_FR; i++)
		{
			sprintf(buf, "%d: %6.3f ", i, kc->A(i));
			console->PrintLine(buf);
		} 
		console->PrintLine("");
	}
	else if(key == ' ')
	{
		solveImmedietely = !solveImmedietely;
		if(solveImmedietely){
			SolveReverseKinematicProblem(KinematicChain::getSingletonPtr(), Effector::getSingletonPtr());
		}
	}
	else if(key == 'r')
	{
		KinematicChain *kc = KinematicChain::getSingletonPtr();
		for(int i = 0; i < DEG_FR; i++)
		{
			kc->A(i) = 1;
		}
	}
	else if(key == 'R')
	{
		recalculateAproxSolver = true;
		KinematicChain::getSingleton().Load(fileNames[currentFile].c_str());
	}
	else if(key == 'u')
	{
		useAproxSolver = !useAproxSolver;
	}
	else if(key == '+')
	{
		recalculateAproxSolver = true;
		currentFile++;
		currentFile %= fileNames.size();
		glutSetWindowTitle(fileNames[currentFile].c_str());
		KinematicChain::getSingleton().Load(fileNames[currentFile].c_str());
	}
	else if(key == '-')
	{
		recalculateAproxSolver = true;
		if(currentFile == 0)
		{
			currentFile = fileNames.size() - 1;
		}
		else
		{
			currentFile--;
		}
		currentFile %= fileNames.size();
		glutSetWindowTitle(fileNames[currentFile].c_str());
		KinematicChain::getSingleton().Load(fileNames[currentFile].c_str());
	}
	else if(key == ',')
	{
		firstSolution = !firstSolution;
	}
	else if(key == '.')
	{
		KinematicChain::getSingleton().InteractiveMode = !KinematicChain::getSingleton().InteractiveMode;
	}
	else if(key == 'z')
	{
		GLConsole *console = GLConsole::getSingletonPtr();
		console->Visible = !console->Visible;
	}
}


void SpecialKeyPressed(int key, int x, int y) 
{
	Effector *eff = Effector::getSingletonPtr();
	if(eff->IsSelected){
		mtxlib::vector4 vecX(0.1, 0, 0, 0);
		mtxlib::vector4 vecY(0, 0.1, 0, 0);
		mtxlib::vector4 vecZ(0, 0, 0.1, 0);

		mtxlib::matrix44 camMat = mtxlib::IdentityMatrix44(); //Camera::getSingleton().GetRotMatrix().invert();
		vecX = camMat * vecX;
		vecY = camMat * vecY;
		vecZ = camMat * vecZ;

		switch(key) {
		case GLUT_KEY_UP:
			if(glutGetModifiers() != GLUT_ACTIVE_CTRL)
				Effector::getSingletonPtr()->Translate(vecY.x, vecY.y, vecY.z);
			else 
				Effector::getSingletonPtr()->Translate(vecZ.x, vecZ.y, vecZ.z);
			break;
		case GLUT_KEY_DOWN:
			if(glutGetModifiers() != GLUT_ACTIVE_CTRL)
				Effector::getSingletonPtr()->Translate(-vecY.x, -vecY.y, -vecY.z);
			else 
				Effector::getSingletonPtr()->Translate(-vecZ.x, -vecZ.y, -vecZ.z);
			break;

			break;
		case GLUT_KEY_LEFT:
			Effector::getSingletonPtr()->Translate(-vecX.x, -vecX.y, -vecX.z);
			break;
		case GLUT_KEY_RIGHT:
			Effector::getSingletonPtr()->Translate(vecX.x, vecX.y, vecX.z);
			break;
		}
		if(solveImmedietely){
			SolveReverseKinematicProblem(KinematicChain::getSingletonPtr(), Effector::getSingletonPtr());
		}
	}
}


void MouseMovePressed(int x,int y)
{
	mouseState.rx = (x - mouseState.ox);
	mouseState.ry = (y - mouseState.oy);

	Effector *eff = Effector::getSingletonPtr();
	KinematicChain *kc = KinematicChain::getSingletonPtr();

	if(mouseState.left)
	{
		
			Camera::getSingleton().RotateX(mouseState.ry);
			Camera::getSingleton().RotateY(mouseState.rx);
	}
	if(mouseState.right)
	{
				
		if(!eff->IsSelected && !kc->IsSelected)
		{
		Camera::getSingleton().Translate(mouseState.ry);
		}
		else if(kc->IsSelected)
		{
			if(kc->SelectedJoint() >= 0)
				kc->A(kc->SelectedJoint()) += -0.001 * (mouseState.ry);

		}
		else if(eff->IsSelected)
		{
			mtxlib::vector3 vecX(1, 0, 0);
			mtxlib::vector3 vecY(0, 1, 0);
			
			mtxlib::matrix44 effMat = Camera::getSingleton().GetMatrix().invert();
			vecX = effMat * vecX;
			vecY = effMat * vecY;

			Effector::getSingleton().RotateVec(vecX, mouseState.ry);
			Effector::getSingleton().RotateVec(vecY, mouseState.rx);

			if(solveImmedietely){
				SolveReverseKinematicProblem(KinematicChain::getSingletonPtr(), Effector::getSingletonPtr());
			}
		}
	}
	if(mouseState.middle)
	{
		
	}

	mouseState.ox = x;
	mouseState.oy = y;
}


void MouseButtonPressed(int button, int state, int x, int y)
{	
	mouseState.ox = x;
	mouseState.oy = y;
	

	switch(button)
	{
	case GLUT_LEFT_BUTTON:
		if(state == GLUT_DOWN)
		{
			mouseState.left = true;
		}
		else
		{
			mouseState.left = false;
		}
		break;
		break;
	case GLUT_RIGHT_BUTTON:
		if(state == GLUT_DOWN)
		{
			mouseState.right = true;
		}
		else
		{
			mouseState.right = false;
		}
		break;
	case GLUT_MIDDLE_BUTTON:
		if(state == GLUT_DOWN)
		{
			ProcessPicing(x, y);
			mouseState.middle = true;
		}
		else
		{
			mouseState.middle = false;
		}
		break;
	}
}


void ReSizeGLScene(int Width, int Height)
{
	if (Height == 0)                                
		Height = 1;

	glViewport(0, 0, Width, Height);                

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	gluPerspective(45.0f, (GLfloat)Width/(GLfloat)Height, 0.1f, 100.0f);
	glMatrixMode(GL_MODELVIEW);
}


std::pair<mtxlib::matrix44, mtxlib::vector3> EffectorDataGenerator()
{
	std::pair<mtxlib::matrix44, mtxlib::vector3> ret_val;
	static int i = 0;
	i++;
	if(i == EFFDATANUM)
	{
		i = 0;
	}
	const float radius = 2;
	mtxlib::matrix44 mat = mtxlib::RotateRadMatrix44('z', 4*PI*(double)i/EFFDATANUM) * mtxlib::RotateRadMatrix44('x', 8*PI*(double)i/EFFDATANUM) * mtxlib::RotateRadMatrix44('z', 2*PI*(double)i/EFFDATANUM);
	ret_val.first = mat;
	ret_val.second = mtxlib::vector3(radius*sin(2*PI*i/EFFDATANUM), radius*cos(2*PI*i/EFFDATANUM), radius*sin(2*PI*i/EFFDATANUM));
	return ret_val;
}


void IdleFunc()
{
	KinematicChain *kc = KinematicChain::getSingletonPtr();
	if(!kc->InteractiveMode)
	{
		Effector *eff = Effector::getSingletonPtr();
		std::pair<mtxlib::matrix44, mtxlib::vector3> currEff = EffectorDataGenerator();
			
		eff->RotMatrix() = currEff.first;
		eff->SetPos(currEff.second.x, currEff.second.y, currEff.second.z);
		
		SolveReverseKinematicProblem(kc, eff);
	}
	///Sleep(100);
	glutPostRedisplay();
}


void LoadEffectorData()
{
	for(int i = 0; i < EFFDATANUM; i++)
	{
		mtxlib::matrix44 mat = mtxlib::RotateRadMatrix44('x', 2*PI*(double)i/EFFDATANUM) * mtxlib::RotateRadMatrix44('z', 2*PI*(double)i/EFFDATANUM);
		effectorData[i].second = mat.quaternion();
		effectorData[i].first = mtxlib::vector4(2*sin(2*PI*i/EFFDATANUM), 2*cos(2*PI*i/EFFDATANUM), 0, 0);
	}
}


int main(int argc, char **argv)
{
	HANDLE hFind;
	WIN32_FIND_DATAA ffd;
	
	std::string fext = "/*.chn", bdir = "chains";

	if(argc > 1)
	{
		bdir = argv[1];
	}

	hFind = FindFirstFileA((bdir + fext).c_str(), &ffd);
	if(hFind != INVALID_HANDLE_VALUE) 
	{
		do
		{
			printf("%s\n", ffd.cFileName);
			
			std::string s = std::string(bdir.c_str());
			s += "/";
			s += std::string(ffd.cFileName);
			fileNames.push_back(s);
		}while(FindNextFileA(hFind, &ffd) != 0);
	}

	glutInit(&argc, argv);
	glutInitWindowSize(800, 600);
	glutInitWindowPosition(0, 0);
	glutInitDisplayString("alpha>=8 rgb stencil >= 8 double depth>=24");
	int window = glutCreateWindow("postprocgen");
	
	glutDisplayFunc(DrawGLScene);
	glutReshapeFunc(ReSizeGLScene);
	glutKeyboardFunc(KeyPressed);        
	glutMotionFunc(MouseMovePressed);               
	glutMouseFunc(MouseButtonPressed);       
	glutSpecialFunc(SpecialKeyPressed);   
	glutIdleFunc(IdleFunc);
	
	InitGL();

	Camera cm = Camera();
		
	LoadEffectorData();

	/* Wczytanie przykladowego lancucha */
	KinematicChain kc = KinematicChain();
	kc.InteractiveMode = true;
	kc.Load(fileNames[currentFile].c_str());
	glutSetWindowTitle(fileNames[currentFile].c_str());
	kc.SetRenderFunction(DisplayChain<false>);
	kc.SetPicRenderFunction(DisplayChain<true>);
	kc.Visible = true;
	kc.ColorId = 200;
	renderableObjs.push_back(&kc);
	

	Effector eff = Effector();
	eff.SetRenderFunction(DisplayEffector<false>);
	eff.SetPicRenderFunction(DisplayEffector<true>);
	eff.Visible = true;
	eff.ColorId = 20;
	renderableObjs.push_back(&eff);


	Grid grid = Grid();
	grid.SetRenderFunction(DrawGrid<false>);
	grid.SetPicRenderFunction(DrawGrid<true>);
	grid.Visible = true;
	grid.ColorId = 30;
	renderableObjs.push_back(&grid);
	
	GLubyte bytes[128 * 128 * 3];
	GLubyte picBytes[sizeof(bytes)];
	for(int i = 0; i < 128 * 128 * 3; i++)
	{
		bytes[i] = (i*i)&255;
		picBytes[i] = 40;
	}
	
	int width, height, compo;
	GLubyte *camPixels;
	ReadJpg("media\\m_logo.jpg", &width, &height, &camPixels, &compo);
		
	FlatObject logo = FlatObject(camPixels, -74, 10, width, height);
	logo.Visible = true;
	renderableObjs.push_back(&logo);

	GLConsole console = GLConsole(10, 310, 370, 300);
	console.Visible = true;
	renderableObjs.push_back(&console);

	glutMainLoop();

	glutDestroyWindow(window);

	return 1;
}